import numpy as np
import cv2
from time import time
import rclpy
from rclpy.node import Node
import cv2
from std_msgs.msg import Float32MultiArray 
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Int32
from std_msgs.msg import String
import math
import datetime
import time
from threading import Thread
import keyboard
import sys
import tty
import termios


class PixelToWorld(Node):
    def __init__(self):
        super().__init__('pixel_to_world_node')

        self.K = np.array([
        [310.34354645, 0., 306.92561716],
        [0., 310.69929111, 222.28407085],
        [0., 0., 1.]
        ])
        # self.K = np.array([
        # [301.60299848, 0., 305.29203665],
        # [0., 301.71549564, 225.86466635],
        # [0., 0., 1.]
        # ])
        self.rvec = np.array([[-0.02898],[-2.2497383 ],[-2.11757169]])
        self.tvec = np.array([[0.08871559],[0.11155042],[0.15544683]])

        self.K_inv = np.linalg.inv(self.K)
        self.R, _ = cv2.Rodrigues(self.rvec)
        self.T = self.tvec
        self.H = self.K @ np.hstack((self.R[:, :2], self.tvec))  # 只取 r1, r2 和 t，构成单应矩阵 H
        self.H_inv = np.linalg.inv(self.H)

        self.zhuitong_pixel = np.array([[0.0],[0.0],[0.0],[0.0]])
        self.zhuitong_relative = np.array([[0.0],[0.0],[0.0],[0.0]])
        self.park_pixel = np.array([[0.0],[0.0],[0.0],[0.0]])
        self.park_relative = np.array([[0.0],[0.0],[0.0],[0.0]])
        
        self.subscription_cone = self.create_subscription(  
                    Int32MultiArray,  
                    '/cone',  
                    self.cone_callback,  
                    3)
        
        self.subscription_park = self.create_subscription(
                    Int32MultiArray,  
                    '/park',  
                    self.park_callback,  
                    3)
        
        self.cone_publisher = self.create_publisher(Float32MultiArray, '/cone_relative', 3)  # 创建新的发布者，用于发布x和y坐标
        self.park_publisher = self.create_publisher(Float32MultiArray, '/park_relative', 3)  # 创建新的发布者，用于发布x和y坐标
        
    def map(self, u, v):
        pixel = np.array([u, v, 1.0])
        world_h = self.H_inv @ pixel
        world_h /= world_h[2]
        X, Y = world_h[0], world_h[1]
        return X, Y  # 地面坐标 Z=0 上的点

    def map_to_camera(self, u, v):
        """
        将像素点 (u, v) 映射到以相机为原点的地面坐标系（即小车坐标系）
        :return: (x_cam, y_cam)
        """
        Xw, Yw = self.map(u, v)  # 地面（棋盘）坐标系下的点
        Pw = np.array([[Xw], [Yw], [0.0]])  # Z=0 地面点
        #Pw = np.array([[Xw], [0.0], [Yw]])

        # 正确的转换方式：Pc = R.T @ (Pw - T)
        Pc = self.R.T @ (Pw - self.T)

        x_cam = Pc[0, 0]
        y_cam = Pc[2, 0]
        return x_cam, y_cam
    
    def cone_callback(self, msg):
        self.zhuitong_pixel = msg.data

        x_cam, y_cam = self.map_to_camera(self.zhuitong_pixel[0], self.zhuitong_pixel[1])
        x_cam = round(x_cam, 4)
        y_cam = round(y_cam, 4)
        x_cam_1 = -round(100.0*(x_cam - 0.0037), 2)
        y_cam_1 = round(100.0*(y_cam + 0.1317 * 2), 2)
 
        relative_msg = Float32MultiArray()
        relative_msg.data = [y_cam_1, x_cam_1]
        self.cone_publisher.publish(relative_msg)
        #self.get_logger().info(f"像素 ({self.zhuitong_pixel[0]}, {self.zhuitong_pixel[1]}) 映射到以相机为原点（校正前）的地面坐标：X = {100.0*x_cam:.4f} cm, Y = {100*y_cam:.4f} cm")
        self.get_logger().info(f"像素 ({self.zhuitong_pixel[0]}, {self.zhuitong_pixel[1]}) 映射到以相机为原点（校正后）的地面坐标：X = {x_cam_1:.4f} cm, Y = {y_cam_1:.4f} cm")

    def park_callback(self, msg):
        self.park_pixel = msg.data

        x_cam, y_cam = self.map_to_camera(self.park_pixel[0], self.park_pixel[1])
        x_cam = round(x_cam, 4)
        y_cam = round(y_cam, 4)
        x_cam_1 = -round(100.0*(x_cam - 0.0037), 2)
        y_cam_1 = round(100.0*(y_cam + 0.1317 * 2), 2)

        relative_msg = Float32MultiArray()
        relative_msg.data = [y_cam_1, x_cam_1]
        self.park_publisher.publish(relative_msg)
        self.get_logger().info(f"像素 ({self.zhuitong_pixel[0]}, {self.zhuitong_pixel[1]}) 映射到以相机为原点（校正后）的地面坐标：X = {x_cam_1:.4f} cm, Y = {y_cam_1:.4f} cm")
        
        
def main(args=None):  
    rclpy.init(args=args)   
    pixeltoworld= PixelToWorld()  
    rclpy.spin(pixeltoworld)
    pixeltoworld.destroy_node()  
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数





